
/*
*   接收按键参数
*   按得越久调节越快
*   稳定后停止PID调节, 当偏差过大时, 延时开始PID调节
*   稳定后记录上次设置角度
*/
#include <stdio.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_attr.h"

#include "driver/mcpwm.h"
#include "soc/mcpwm_periph.h"
#include "bsp_motor.h"
#include "esp_log.h"
#include "lib_pid.h"
#include "bsp_adc.h"
#include "bsp_sleep.h"

#define GPIO_PWM1A_OUT 26
#define GPIO_PWM1B_OUT 25

#define GPIO_MOTOR_OUT_PIN_SEL ((1ULL << GPIO_PWM1A_OUT) | (1ULL << GPIO_PWM1B_OUT))

#define GPIO_BUTTON_UP 4
#define GPIO_BUTTON_DOWN 16
#define GPIO_BUTTON_IN_PIN_SEL ((1ULL << GPIO_BUTTON_UP) | (1ULL << GPIO_BUTTON_DOWN))

void brushed_motor_gpio_init(void)
{
    gpio_config_t io_conf;
    io_conf.intr_type = GPIO_INTR_DISABLE;
    io_conf.mode = GPIO_MODE_OUTPUT;
    io_conf.pin_bit_mask = GPIO_MOTOR_OUT_PIN_SEL;
    io_conf.pull_down_en = 0;
    io_conf.pull_up_en = 0;
    gpio_config(&io_conf);

    io_conf.intr_type = GPIO_INTR_DISABLE; //GPIO_INTR_NEGEDGE;
    io_conf.pin_bit_mask = GPIO_BUTTON_IN_PIN_SEL;
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pull_up_en = 1;
    gpio_config(&io_conf);
    gpio_set_level(GPIO_PWM1A_OUT, 1);
    gpio_set_level(GPIO_PWM1B_OUT, 1);
}

void brushed_motor_control(uint8_t control)
{
    switch (control)
    {
    case MOTOR_LOW_POWER:
        gpio_set_level(GPIO_PWM1A_OUT, 0);
        gpio_set_level(GPIO_PWM1B_OUT, 0);
        break;
    case MOTOR_FORWARD:
        gpio_set_level(GPIO_PWM1A_OUT, 1);
        gpio_set_level(GPIO_PWM1B_OUT, 0);
        break;
    case MOTOR_BACKWARD:
        gpio_set_level(GPIO_PWM1A_OUT, 0);
        gpio_set_level(GPIO_PWM1B_OUT, 1);
        break;
    case MOTOR_BRAKE:
    default:
        gpio_set_level(GPIO_PWM1A_OUT, 1);
        gpio_set_level(GPIO_PWM1B_OUT, 1);
        //          break;
    }
}

uint8_t brushed_motor_button(void)
{
    uint8_t button_num = BUTTON_NULL;
    if (gpio_get_level(GPIO_BUTTON_UP) == 0)
    {
        button_num = BUTTON_UP;
    }
    else if (gpio_get_level(GPIO_BUTTON_DOWN) == 0)
    {
        button_num = BUTTON_DOWN;
    }
    return button_num;
}

void motor_control_task(void *arg)
{
    brushed_motor_gpio_init();
    while (1)
    {
        uint8_t button_num = brushed_motor_button();

        if (button_num == BUTTON_UP)
        {
            brushed_motor_control(MOTOR_FORWARD);
        }
        else if (button_num == BUTTON_DOWN)
        {
            brushed_motor_control(MOTOR_BACKWARD);
        }
        else
        {
            brushed_motor_control(MOTOR_BRAKE);
        }

        vTaskDelay(10 / portTICK_RATE_MS);
    }
}

void motor_control_task_old(void *arg)
{
    // uint32_t adc_buffer[5] = {0};
    uint8_t control, button_num;
    pid_float pid;
    PID_init(&pid);
    brushed_motor_gpio_init();

    while (1)
    {
        pid.ActualAngle = (float)bsp_adc_read(ADC_ANGLE);
        PID_realize(&pid);
        if (pid.delay_ms > 0)
        {
            if (pid.ActualAngle < 900)
                control = MOTOR_FORWARD;
            else
                control = MOTOR_BRAKE;
        }
        else
        {
            if (pid.ActualAngle > 400)
                control = MOTOR_BACKWARD;
            else
                control = MOTOR_BRAKE;
            pid.delay_ms = -pid.delay_ms;
        }
        if (pid.delay_ms < 5)
        {
            control = MOTOR_BRAKE;
        }

        brushed_motor_control(control);
        // ESP_LOGI(TAG, "ActualAngle:%f err%f delay_ms:%f control:%d\n", pid.ActualAngle, pid.err, pid.delay_ms, control);
        vTaskDelay((uint32_t)pid.delay_ms / portTICK_PERIOD_MS);

        brushed_motor_control(MOTOR_LOW_POWER);
        vTaskDelay(10 / portTICK_PERIOD_MS);

        button_num = brushed_motor_button();
        if (button_num == BUTTON_UP)
        {
            pid.SetAngle += 1;
            if (pid.SetAngle > 900)
                pid.SetAngle = 900;

            bsp_sleep_save_data(SAVE_ANGLE);
        }
        else if (button_num == BUTTON_DOWN)
        {
            pid.SetAngle -= 3;
            if (pid.SetAngle < 400)
                pid.SetAngle = 400;

            bsp_sleep_save_data(SAVE_ANGLE);
        }

        if (button_num == BUTTON_UP)
        {
            if (pid.ActualAngle < 900)
                brushed_motor_control(MOTOR_FORWARD);
            else
                brushed_motor_control(MOTOR_BRAKE);
        }
        else if (button_num == BUTTON_DOWN)
        {

            if (pid.ActualAngle > 400)
                brushed_motor_control(MOTOR_BACKWARD);
            else
                brushed_motor_control(MOTOR_BRAKE);
        }
        else
        {
            brushed_motor_control(MOTOR_BRAKE);
        }

        vTaskDelay(100 / portTICK_RATE_MS);
    }
}